#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
角速度标定程序

该程序用于分析机器人角速度和电机指令值的关系，支持处理RTK和IMU两种传感器数据。
通过对稳态数据的提取和分析，建立电机指令值到角速度的多项式回归模型。
"""

# 导入必要的库
import os  # 操作系统接口，用于文件路径处理
import re  # 正则表达式库，用于文件名解析
import logging  # 日志记录
import numpy as np  # 数值计算
import pandas as pd  # 数据处理
import matplotlib as mpl  # 绘图库
# 设置Agg后端，确保在无GUI环境下也能正常生成图像
mpl.use('Agg')
import matplotlib.pyplot as plt  # 绘图函数
from concurrent.futures import ThreadPoolExecutor  # 多线程处理
from sklearn.model_selection import train_test_split  # 数据集划分
from sklearn.linear_model import LinearRegression  # 线性回归
from sklearn.preprocessing import PolynomialFeatures  # 多项式特征
from datetime import datetime  # 日期时间处理
import joblib  # 模型序列化

# 数据路径设置
BASE_DIR = os.path.dirname(os.path.abspath(__file__))  # 当前脚本所在目录

# 数据配置
# 定义数据源、文件格式和处理方式
DATA_CONFIG = {
    'data_folders': [
        {
            'path': os.path.join(BASE_DIR, "../rtk角速度数据"),  # RTK数据文件夹路径
            'name': "RTK角速度实验",  # 数据集名称，用于日志和输出文件
            'column': 'RTK_Heading',  # 用于计算角速度的列名
            'prefix': 'rtk_'  # 文件名前缀，用于识别文件类型
        },
        {
            'path': os.path.join(BASE_DIR, "../imu角速度数据"),  # IMU数据文件夹路径
            'name': "IMU角速度实验",  # 数据集名称
            'column': 'IMU_Gyro_Z',  # 使用IMU的Z轴角速度
            'prefix': 'imu_'  # 文件名前缀
        }
    ],
    'combined': True,  # 是否合并所有文件夹的数据进行分析
    'separate': True,  # 是否单独分析每个文件夹的数据
}

# 数据处理参数
# 控制稳态点识别和数据过滤的参数
PROCESSING_CONFIG = {
    'window_size_ratio': 0.1,  # 滑动窗口大小比例，用于平滑数据计算稳态
    'diff_threshold_ratio': 0.1,  # 角速度差分阈值比例，用于判断稳态
    'min_data_points': 10,  # 最小数据点数，低于此值的数据会被忽略
}

# 模型参数
# 控制多项式回归模型的训练参数
MODEL_CONFIG = {
    'degree': 2,  # 多项式回归次数，一般使用2次即可，次数过高可能过拟合
    'test_size': 0.2,  # 测试集比例，用于模型验证
    'random_state': 42,  # 随机种子，确保结果可重复
    'cv_folds': 5,  # 交叉验证折数，用于评估模型稳定性
}

# 可视化参数
# 控制图表的生成参数
VISUALIZATION_CONFIG = {
    'figsize': (10, 6),  # 图表大小
    'dpi': 100,  # 分辨率
    'save_path': 'plots',  # 保存路径
    'combined_plot': True,  # 是否生成合并数据的图表
}

# 输出配置
# 控制结果文件的命名和存储
OUTPUT_CONFIG = {
    'results_file': 'angular_velocity_calibration_results.csv',  # 标定结果文件
    'combined_results_file': 'combined_angular_velocity_calibration_results.csv',  # 合并标定结果
    'model_file': 'angular_velocity_model.joblib',  # 模型文件
    'poly_file': 'angular_velocity_poly_features.joblib',  # 多项式特征转换器文件
    'log_file': 'angular_velocity_calibration.log',  # 日志文件
}

def setup_logging(log_file):
    """设置日志配置
    
    初始化日志系统，设置日志格式和输出方式。
    
    参数:
        log_file: 日志文件路径
    """
    os.makedirs(os.path.dirname(log_file), exist_ok=True)  # 确保日志目录存在
    logging.basicConfig(
        level=logging.INFO,  # 设置日志级别为INFO
        format='%(asctime)s - %(levelname)s - %(message)s',  # 设置日志格式：时间-级别-消息
        handlers=[
            logging.FileHandler(log_file),  # 文件处理器，将日志写入文件
            logging.StreamHandler()  # 流处理器，将日志输出到控制台
        ]
    )

def extract_command_from_filename(filename, prefix):
    """从文件名中提取命令值
    
    解析文件名，提取电机命令值。文件名格式应为: [prefix]_[命令值]_[命令值].csv
    
    参数:
        filename: 文件名
        prefix: 文件名前缀，如"rtk_"或"imu_"
        
    返回:
        int: 提取的命令值，失败则返回None
    """
    try:
        # 从文件路径中提取文件名
        base_name = os.path.basename(filename)
        # 移除扩展名
        name_without_ext = os.path.splitext(base_name)[0]
        # 如果有前缀，移除前缀
        if prefix and name_without_ext.startswith(prefix):
            name_without_prefix = name_without_ext[len(prefix):]
        else:
            name_without_prefix = name_without_ext
            
        # 解析命令值，格式为"命令值_命令值"，取第一个值
        cmd_parts = name_without_prefix.split('_')
        if len(cmd_parts) >= 1:
            return int(cmd_parts[0])
    except Exception as e:
        logging.error(f"从文件名 {filename} 提取命令时出错: {e}")
    
    return None

def calculate_angular_velocity(df, column_name):
    """计算角速度
    
    根据传感器数据计算角速度，处理方式取决于传感器类型。
    - RTK: 根据航向角和时间差计算角速度，处理0-360度的跳变
    - IMU: 直接使用IMU提供的角速度值
    
    参数:
        df: 数据帧，包含传感器数据
        column_name: 用于计算角速度的列名
        
    返回:
        DataFrame: 添加了Angular_Velocity列的数据帧
    """
    if column_name == 'RTK_Heading':
        # 对于RTK数据，我们需要计算角速度
        # 首先处理航向角的突变（0-360度的跳变）
        df['Heading_Adjusted'] = df[column_name].copy()
        for i in range(1, len(df)):
            # 计算相邻点的航向角差值
            diff = df['Heading_Adjusted'].iloc[i] - df['Heading_Adjusted'].iloc[i-1]
            # 处理0-360度跳变：当差值大于180度，认为过了0度
            if diff > 180:
                df.loc[df.index[i:], 'Heading_Adjusted'] -= 360
            # 当差值小于-180度，认为过了360度
            elif diff < -180:
                df.loc[df.index[i:], 'Heading_Adjusted'] += 360
        
        # 计算角速度 (度/秒)：航向角差值除以时间差
        df['Angular_Velocity'] = df['Heading_Adjusted'].diff() / df['Timestamp'].diff()
        # 填充第一行的NaN值
        df['Angular_Velocity'].iloc[0] = 0
        
        return df
    elif column_name == 'IMU_Gyro_Z':
        # 对于IMU数据，直接使用IMU_Gyro_Z作为角速度
        # 注意：可能需要根据实际IMU数据的单位进行换算
        df['Angular_Velocity'] = df[column_name]
        return df
    else:
        # 如果是其他类型的数据，可以添加相应的处理方法
        logging.warning(f"未定义的数据列名: {column_name}，无法计算角速度")
        df['Angular_Velocity'] = 0  # 设置为默认值0
        return df

def process_file(file_path, prefix):
    """处理单个数据文件
    
    读取CSV文件，提取命令值，计算角速度，并格式化数据。
    
    参数:
        file_path: CSV文件路径
        prefix: 文件名前缀
        
    返回:
        DataFrame: 处理后的数据帧，包含角速度和命令值
    """
    try:
        # 从文件名中提取命令值
        command = extract_command_from_filename(file_path, prefix)
        if command is None:
            logging.error(f"无法从文件名 {file_path} 中提取命令值")
            return pd.DataFrame()  # 返回空数据帧
            
        # 读取CSV文件
        df = pd.read_csv(file_path)
        
        # 验证必要的列是否存在
        if 'Timestamp' not in df.columns or 'Motor_Speed' not in df.columns:
            logging.error(f"文件 {file_path} 中缺少必要的列")
            return pd.DataFrame()  # 返回空数据帧
            
        # 判断是RTK还是IMU数据，并计算角速度
        if 'RTK_Heading' in df.columns:
            df = calculate_angular_velocity(df, 'RTK_Heading')
        elif 'IMU_Gyro_Z' in df.columns:
            df = calculate_angular_velocity(df, 'IMU_Gyro_Z')
        else:
            logging.error(f"文件 {file_path} 中缺少角速度相关列")
            return pd.DataFrame()  # 返回空数据帧
            
        # 添加命令列，用于后续分析
        df['Command'] = command
        
        logging.info(f"成功处理文件 {file_path}, 数据点数: {len(df)}")
        return df
        
    except Exception as e:
        logging.error(f"处理文件 {file_path} 时出错: {e}")
        return pd.DataFrame()  # 返回空数据帧

def calculate_optimal_window_size(df, ratio):
    """计算最优滑动窗口大小
    
    根据数据点数量和配置的比例计算窗口大小。
    
    参数:
        df: 数据帧
        ratio: 窗口大小比例
        
    返回:
        int: 窗口大小，至少为5
    """
    return max(int(len(df) * ratio), 5)  # 最小窗口大小为5

def calculate_optimal_threshold(df, ratio):
    """计算最优角速度差分阈值
    
    根据角速度数据的标准差和配置的比例计算阈值。
    
    参数:
        df: 数据帧
        ratio: 阈值比例
        
    返回:
        float: 角速度差分阈值
    """
    return df['Angular_Velocity'].std() * ratio  # 使用标准差乘以比例

def find_steady_state(df, window_size, diff_threshold):
    """查找稳态数据
    
    通过分析角速度的变化率，识别机器人处于稳定角速度状态的数据点。
    
    实现步骤:
    1. 计算相邻角速度值的差分
    2. 使用滑动窗口计算平均差分
    3. 识别平均差分低于阈值的点为稳态点
    4. 如果稳态点过少，放宽条件选择差分最小的点
    
    参数:
        df: 数据帧，包含角速度数据
        window_size: 滑动窗口大小
        diff_threshold: 角速度差分阈值
        
    返回:
        DataFrame: 仅包含稳态数据点的数据帧
    """
    # 计算角速度差分的绝对值，表示相邻时刻角速度的变化量
    df['Velocity_Diff'] = df['Angular_Velocity'].diff().abs()
    
    # 使用滚动窗口计算平均差分，使结果更平滑，抑制短期波动影响
    df['Avg_Abs_Diff'] = df['Velocity_Diff'].rolling(window=window_size, center=True).mean()
    
    # 填充 NaN 值，避免数据丢失（窗口两端的数据点）
    df['Avg_Abs_Diff'] = df['Avg_Abs_Diff'].fillna(df['Velocity_Diff'].mean())
    
    # 如果阈值太小，可能导致没有稳态点，调整阈值
    if diff_threshold < 0.001:
        diff_threshold = 0.001
    
    # 寻找满足稳态条件的点：平均差分小于等于阈值
    steady_state_mask = df['Avg_Abs_Diff'] <= diff_threshold
    steady_state_df = df[steady_state_mask]
    
    # 如果稳态点太少，放宽条件
    if len(steady_state_df) < 10:
        # 尝试使用前25%的最小变化点作为稳态点
        sorted_diff = df.sort_values(by='Avg_Abs_Diff')  # 按差分大小排序
        num_points = max(10, int(len(df) * 0.25))  # 至少10个点或25%的点
        steady_state_df = sorted_diff.head(num_points)  # 取差分最小的一部分点
        logging.info(f"稳态点太少，已放宽条件选取前{num_points}个最小变化点")
    
    return steady_state_df

def plot_angular_velocity_data(df, steady_state_df, steady_state_velocity, command, save_path):
    """绘制角速度数据图
    
    创建角速度随时间变化的图表，展示原始数据、稳态数据点和平均稳态角速度。
    
    参数:
        df: 原始数据帧
        steady_state_df: 稳态数据帧
        steady_state_velocity: 稳态角速度平均值
        command: 电机命令值
        save_path: 图表保存路径
    """
    # 设置matplotlib后端为Agg
    mpl.use('Agg')
    
    # 创建新图表
    plt.figure(figsize=VISUALIZATION_CONFIG['figsize'], 
               dpi=VISUALIZATION_CONFIG['dpi'])
    
    # 转换时间戳为数值索引以避免多维索引问题
    time_indices = np.arange(len(df))
    
    # 获取当前时间作为时间戳，用于图表标题
    current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
    
    # 绘制原始角速度曲线
    plt.plot(time_indices, df['Angular_Velocity'].values, 'b-', 
             label='Angular Velocity', alpha=0.5)
    
    # 如果有稳态数据，绘制稳态角速度曲线和平均值
    if not steady_state_df.empty:
        # 找到稳态数据在原始数据中的索引
        steady_indices = []
        for idx, row in steady_state_df.iterrows():
            if idx in df.index:
                steady_indices.append(df.index.get_loc(idx))
        
        if steady_indices:
            # 绘制稳态角速度点（红色散点）
            steady_x = [time_indices[i] for i in steady_indices]
            steady_y = [df.iloc[i]['Angular_Velocity'] for i in steady_indices]
            plt.scatter(steady_x, steady_y, color='red', s=30, label='Steady State')
            
            # 绘制平均稳态角速度线（绿色虚线）
            plt.axhline(y=steady_state_velocity, color='g', linestyle='--', 
                       label=f'Avg:{steady_state_velocity:.3f} deg/s')
    
    # 添加简洁的标签和标题
    plt.xlabel('Sample Points')  # X轴标签
    plt.ylabel('Angular Velocity (deg/s)')  # Y轴标签
    plt.title(f'Angular Velocity Curve (Command={command})\n{current_time}')  # 标题
    plt.legend()  # 图例
    plt.grid(True)  # 网格
    
    # 保存图片到指定路径
    os.makedirs(save_path, exist_ok=True)  # 确保保存目录存在
    plt.savefig(os.path.join(save_path, f'angular_velocity_plot_{command}.png'))
    plt.close()  # 关闭图表，释放内存

def train_model(X, y):
    """训练多项式回归模型
    
    使用多项式特征转换和线性回归拟合角速度与电机指令值的关系。
    
    参数:
        X: 输入特征（电机指令值）
        y: 目标值（稳态角速度）
        
    返回:
        tuple: (model, poly) - 训练好的线性回归模型和多项式特征转换器
    """
    # 创建多项式特征
    # 次数过高会导致过拟合，通常2-3次即可
    degree = min(MODEL_CONFIG['degree'], len(X) - 1)  # 防止特征数超过样本数导致过拟合
    poly = PolynomialFeatures(degree=degree)
    X_poly = poly.fit_transform(X.reshape(-1, 1))  # 转换为多项式特征
    
    # 训练线性回归模型
    model = LinearRegression()
    model.fit(X_poly, y)  # 拟合模型
    
    return model, poly

def plot_combined_calibration_data(all_datasets, save_path):
    """绘制合并的标定数据图
    
    创建包含所有数据集和拟合曲线的综合图表，用于比较不同数据源的标定结果。
    
    参数:
        all_datasets: 包含所有数据集信息的字典，格式为 {数据集名称: {数据, 模型, 特征转换器}}
        save_path: 图表保存路径
    """
    # 设置matplotlib后端为Agg
    mpl.use('Agg')
    
    # 创建新图表
    plt.figure(figsize=VISUALIZATION_CONFIG['figsize'], 
               dpi=VISUALIZATION_CONFIG['dpi'])
    
    # 获取当前时间作为时间戳
    current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
    
    # 创建曲线拟合对象和颜色映射
    # 为不同数据集分配不同的颜色和标记样式
    colors = ['b', 'r', 'g', 'c', 'm', 'y', 'k']  # 颜色列表
    markers = ['o', 's', '^', 'D', 'v', '<', '>']  # 标记样式列表
    
    # 用于收集所有数据点，以便生成合并拟合曲线
    combined_X = []
    combined_y = []
    
    # 绘制所有数据集
    for i, (dataset_name, dataset) in enumerate(all_datasets.items()):
        # 循环索引到颜色和标记样式列表中
        color_idx = i % len(colors)
        marker_idx = i % len(markers)
        
        # 提取当前数据集的指令值和角速度
        X = dataset['data']['Command'].values
        y = dataset['data']['SteadyStateVelocity'].values
        
        # 绘制数据点
        plt.scatter(X, y, 
                    color=colors[color_idx], 
                    marker=markers[marker_idx],
                    s=50,  # 点大小
                    alpha=0.7,  # 透明度
                    label=f"{dataset_name} Data")
        
        # 如果数据集有对应的模型，绘制拟合曲线
        if 'model' in dataset and 'poly' in dataset:
            # 生成平滑曲线用的x值
            x_fit = np.linspace(min(X), max(X), 100).reshape(-1, 1)
            # 用模型预测y值
            y_fit = dataset['model'].predict(dataset['poly'].transform(x_fit))
            # 绘制拟合曲线
            plt.plot(x_fit, y_fit, 
                     color=colors[color_idx], 
                     linestyle='--',  # 虚线
                     alpha=0.7,
                     label=f"{dataset_name} Fit")
        
        # 收集所有数据点用于合并拟合
        combined_X.extend(X)
        combined_y.extend(y)
    
    # 如果有合并的模型，绘制合并拟合曲线
    if 'combined' in all_datasets and 'model' in all_datasets['combined'] and 'poly' in all_datasets['combined']:
        combined_model = all_datasets['combined']['model']
        combined_poly = all_datasets['combined']['poly']
        
        # 生成平滑的合并拟合曲线
        x_fit = np.linspace(min(combined_X), max(combined_X), 100).reshape(-1, 1)
        y_fit = combined_model.predict(combined_poly.transform(x_fit))
        
        # 绘制合并拟合曲线（黑色实线）
        plt.plot(x_fit, y_fit, 
                 color='black', 
                 linewidth=2,  # 加粗线宽
                 label="Combined Fit")
    
    # 添加图表标题和标签
    plt.title(f"Angular Velocity Calibration Results\n{current_time}")
    plt.xlabel("Command Value")
    plt.ylabel("Angular Velocity (deg/s)")
    plt.grid(True)  # 添加网格线
    plt.legend()  # 添加图例
    
    # 保存图表
    plt.savefig(os.path.join(save_path, "combined_angular_velocity_plot.png"))
    plt.close()  # 关闭图表，释放内存
    logging.info(f"已保存合并数据标定结果图表")

def process_data_folder(folder_config, plots_dir, log_dir):
    """处理单个数据文件夹
    
    读取一个文件夹中的所有CSV文件，进行角速度计算、稳态检测和模型训练。
    
    处理流程:
    1. 读取目录下所有CSV文件
    2. 分析每个文件，提取电机指令值和角速度
    3. 为每个指令值识别稳态数据
    4. 计算稳态角速度
    5. 生成数据可视化图表
    6. 训练角速度模型
    
    参数:
        folder_config: 文件夹配置信息，包含path、name和prefix
        plots_dir: 图表保存目录
        log_dir: 日志和结果保存目录
        
    返回:
        dict: 包含数据、模型和多项式特征转换器的字典，如果处理失败则返回None
    """
    # 获取配置信息
    folder_path = folder_config['path']  # 文件夹路径
    folder_name = folder_config['name']  # 文件夹名称
    prefix = folder_config['prefix']  # 文件前缀
    
    logging.info(f"开始处理数据文件夹: {folder_name} ({folder_path})")
    
    # 创建文件夹特定的输出目录，用于保存图表
    folder_plots_dir = os.path.join(plots_dir, folder_name.replace(" ", "_"))
    os.makedirs(folder_plots_dir, exist_ok=True)
    
    # 获取所有CSV文件
    file_list = [f for f in os.listdir(folder_path) if f.endswith('.csv')]
    
    if not file_list:
        logging.error(f"文件夹 {folder_name} 中没有找到CSV文件")
        return None
    
    # 使用线程池处理文件，加速数据处理
    data = {}  # 存储处理后的数据，格式为 {命令值: 数据帧}
    with ThreadPoolExecutor() as executor:
        # 创建处理任务
        futures = []
        for filename in file_list:
            file_path = os.path.join(folder_path, filename)
            futures.append(executor.submit(process_file, file_path, prefix))

        # 收集处理结果
        for future in futures:
            df = future.result()
            if not df.empty:  # 如果成功处理
                command = df['Command'].iloc[0]  # 提取命令值
                data[command] = df  # 将数据帧存入字典

    # 检查是否成功处理了任何文件
    if not data:
        logging.error(f"文件夹 {folder_name} 中没有找到有效数据")
        return None

    # 处理每个文件的数据，计算稳态角速度
    calibration_data = []  # 存储标定结果 [命令值, 稳态角速度]
    
    # 按命令值顺序处理
    for command, df in sorted(data.items()):
        # 检查数据点是否足够
        if len(df) < PROCESSING_CONFIG['min_data_points']:
            logging.warning(f"指令 {command} 的数据点不足")
            continue

        logging.info(f"处理指令 {command} 的数据，共 {len(df)} 个数据点")

        try:
            # 计算最优参数：窗口大小和差分阈值
            window_size = calculate_optimal_window_size(df, PROCESSING_CONFIG['window_size_ratio'])
            diff_threshold = calculate_optimal_threshold(df, PROCESSING_CONFIG['diff_threshold_ratio'])
            
            logging.info(f"计算参数: window_size={window_size}, diff_threshold={diff_threshold:.6f}")

            # 查找稳态数据
            steady_state_df = find_steady_state(df, window_size, diff_threshold)
            
            # 如果没找到稳态数据，跳过当前命令值
            if steady_state_df.empty:
                logging.warning(f"指令 {command} 没有找到稳态数据，跳过")
                continue

            # 计算稳态角速度：取稳态区间的平均值
            steady_state_velocity = steady_state_df['Angular_Velocity'].mean()
            logging.info(f"指令 {command} 的稳态角速度: {steady_state_velocity:.6f} deg/s")
            
            # 将结果添加到标定数据中
            calibration_data.append([command, steady_state_velocity])

            # 绘制数据可视化图表
            try:
                plot_angular_velocity_data(df, steady_state_df, steady_state_velocity, command, folder_plots_dir)
                logging.info(f"已保存指令 {command} 的角速度曲线图")
            except Exception as e:
                logging.error(f"绘制指令 {command} 的图形时出错: {e}")
                
        except Exception as e:
            logging.error(f"处理指令 {command} 时出错: {e}")
            continue

    # 如果没有找到任何有效的校准数据，返回None
    if not calibration_data:
        logging.error(f"文件夹 {folder_name} 中没有找到任何有效的稳态数据")
        return None
        
    # 创建标定数据DataFrame，用于保存和分析
    calibration_df = pd.DataFrame(calibration_data, columns=['Command', 'SteadyStateVelocity'])
    logging.info(f"文件夹 {folder_name} 的标定数据:\n{calibration_df}")

    # 保存标定结果到CSV文件
    results_file = os.path.join(log_dir, f"{folder_name.replace(' ', '_')}_{OUTPUT_CONFIG['results_file']}")
    calibration_df.to_csv(results_file, index=False)
    logging.info(f"标定结果已保存到 {results_file}")

    # 模型训练
    # 至少需要3个点才能拟合二次曲线
    if len(calibration_df) >= 3:
        X = calibration_df['Command'].values
        y = calibration_df['SteadyStateVelocity'].values

        try:
            # 训练多项式回归模型
            model, poly = train_model(X, y)
            
            # 保存模型和特征转换器
            model_file = os.path.join(log_dir, f"{folder_name.replace(' ', '_')}_{OUTPUT_CONFIG['model_file']}")
            poly_file = os.path.join(log_dir, f"{folder_name.replace(' ', '_')}_{OUTPUT_CONFIG['poly_file']}")
            joblib.dump(model, model_file)
            joblib.dump(poly, poly_file)
            logging.info(f"文件夹 {folder_name} 的模型已保存到 {model_file}")
            
            # 返回数据和模型
            return {
                'name': folder_name,
                'data': calibration_df,
                'model': model,
                'poly': poly
            }
            
        except Exception as e:
            logging.error(f"文件夹 {folder_name} 的模型训练出错: {e}")
            # 返回数据但没有模型
            return {
                'name': folder_name,
                'data': calibration_df
            }
    else:
        logging.warning(f"文件夹 {folder_name} 的数据点不足，无法拟合模型")
        # 返回数据但没有模型
        return {
            'name': folder_name,
            'data': calibration_df
        }

def main():
    """主函数
    
    程序的入口点，执行完整的角速度标定流程。
    
    处理流程：
    1. 初始化日志系统
    2. 创建输出目录
    3. 处理所有配置的数据文件夹
    4. 合并各数据文件夹的结果（如果启用）
    5. 生成综合图表
    """
    # 设置日志
    log_dir = os.path.dirname(os.path.abspath(__file__))
    log_file = os.path.join(log_dir, OUTPUT_CONFIG['log_file'])
    setup_logging(log_file)
    logging.info("开始角速度数据分析")
    
    # 创建输出目录
    plots_dir = os.path.join(log_dir, VISUALIZATION_CONFIG['save_path'])
    os.makedirs(plots_dir, exist_ok=True)

    # 处理所有数据文件夹
    all_datasets = {}  # 存储所有数据集的结果
    all_calibration_data = []  # 存储所有标定点
    
    # 遍历配置中的所有数据文件夹
    for folder_config in DATA_CONFIG['data_folders']:
        # 处理单个文件夹
        if DATA_CONFIG['separate']:
            # 单独分析此数据文件夹
            dataset = process_data_folder(folder_config, plots_dir, log_dir)
            if dataset:
                folder_name = dataset['name']
                all_datasets[folder_name] = dataset
                
                # 收集所有标定数据点
                if 'data' in dataset:
                    for _, row in dataset['data'].iterrows():
                        all_calibration_data.append([
                            row['Command'], 
                            row['SteadyStateVelocity'], 
                            folder_name
                        ])
        else:
            # 即使不单独分析，也需要收集数据用于合并分析
            dataset = process_data_folder(folder_config, plots_dir, log_dir)
            if dataset and 'data' in dataset:
                folder_name = dataset['name']
                for _, row in dataset['data'].iterrows():
                    all_calibration_data.append([
                        row['Command'], 
                        row['SteadyStateVelocity'], 
                        folder_name
                    ])
    
    # 如果需要合并处理
    if DATA_CONFIG['combined'] and all_calibration_data:
        # 创建合并的标定数据DataFrame
        combined_df = pd.DataFrame(all_calibration_data, 
                                 columns=['Command', 'SteadyStateVelocity', 'Dataset'])
        
        # 保存合并标定结果
        combined_results_file = os.path.join(log_dir, OUTPUT_CONFIG['combined_results_file'])
        combined_df.to_csv(combined_results_file, index=False)
        logging.info(f"合并标定结果已保存到 {combined_results_file}")
        
        # 合并模型训练
        if len(combined_df) >= 3:
            X = combined_df['Command'].values
            y = combined_df['SteadyStateVelocity'].values
            
            try:
                # 训练合并模型
                model, poly = train_model(X, y)
                
                # 保存合并模型
                model_file = os.path.join(log_dir, f"combined_{OUTPUT_CONFIG['model_file']}")
                poly_file = os.path.join(log_dir, f"combined_{OUTPUT_CONFIG['poly_file']}")
                joblib.dump(model, model_file)
                joblib.dump(poly, poly_file)
                logging.info(f"合并模型已保存到 {model_file}")
                
                # 添加到数据集合中
                all_datasets['combined'] = {
                    'name': '合并数据',
                    'data': combined_df,
                    'model': model,
                    'poly': poly
                }
                
            except Exception as e:
                logging.error(f"合并模型训练出错: {e}")
        else:
            logging.warning("合并数据点不足，无法拟合模型")
    
    # 绘制合并图表
    if VISUALIZATION_CONFIG['combined_plot'] and len(all_datasets) > 0:
        try:
            plot_combined_calibration_data(all_datasets, plots_dir)
        except Exception as e:
            logging.error(f"绘制合并图表时出错: {e}")
    
    logging.info("角速度数据分析完成")

if __name__ == '__main__':
    try:
        main()
    except Exception as e:
        logging.error(f"程序执行出错: {e}", exc_info=True)
    finally:
        plt.close('all')  # 确保所有图表都被关闭，释放资源 